#ifndef __DRRT_H__
#define __DRRT_H__

#include "basic_typedef.h"
#include <vector>
#include <utility>
#include "Graph.h"
#include "CollisionDetector2D.h"
#include "Kd_tree_d.h"

class Prm2D;
class CollisionDetector;
class Sampler;

typedef std::pair<Point_2, Point_2> PointPair;
// Should be used with the Graph wrapper
struct Less_than_indexPair {
  bool operator()(const PointPair& p1, 
                  const PointPair& p2) const {
	  if (p1.first != p2.first) {
		  return p1.first < p2.first;
	  } else {
		  return p1.second < p2.second;
	  }
  }
};

typedef Graph<PointPair, Less_than_indexPair> CompositeGraph;

class Drrt {
  public:
    Drrt(const Prm2D& robot1Graph,
         const Prm2D& robot2Graph,
         const CollisionDetector2D& robotCollisionDetection,
         const CollisionDetector& compositeCollisionDetector,
         const Sampler& compositeSampler,
         const PointPair& start, 
         const PointPair& target);

    void runLoop(int numIterations);
    vector<PointPair> retrieve_path();

  private:
    void expand(CompositeGraph& graph, Kd_tree_d<Kernel_d>& kdTree);
    vector<PointPair> connectTrees();
    vector<PointPair> localConnector(PointPair start, PointPair goal) const;
    vector<PointPair> reconstructPath(map<PointPair, PointPair> cameFrom, PointPair start, PointPair goal) const;
    vector<PointPair> getNeighbors(PointPair current) const;

    PointPair compositeDirectionOracle(
        const CompositeGraph& graph, const Point_d& qRand, const PointPair& qNear);
    Point_2 directionOracle(const Graph<Point_2>& graph, Point_2 qNear, Point_2 qRand);
    PointPair nearestNeighbor(Kd_tree_d<Kernel_d>& kdTree, Point_d point);

    Point_d pointDFromPointPair(const PointPair& indexPair) const;
    PointPair pointPairFromPointD(const Point_d& point) const;

    double configurationDistance(const PointPair& a, const PointPair& b) const;

    vector<PointPair> generatePath(vector<PointPair>& treeConnection); 

  private:
    const Prm2D* m_robot1Prm;
    const Prm2D* m_robot2Prm;
    const CollisionDetector2D* m_robotCollisionDetection;
    const CollisionDetector* m_compositeCollisionDetection;
    const Sampler* m_compositeSampler;

    const PointPair* m_start;
    const PointPair* m_target;

    CompositeGraph m_startGraph;
    Kd_tree_d<Kernel_d> m_startKd;
    CompositeGraph m_targetGraph;
    Kd_tree_d<Kernel_d> m_targetKd;

    vector<PointPair> m_bestPath;
};

#endif
